#pragma once

#include <opencv2/opencv.hpp>

// 位置解算类
class PoseSolver {
private:
  cv::Mat camera_matrix;                  // 相机内参矩阵
  cv::Mat dist_coeffs;                    // 相机畸变参数
  cv::Mat rvec;                           // 旋转向量
  cv::Mat tvec;                           // 平移向量
  std::vector<cv::Point3f> object_points; // 3D 物体点

public:
  PoseSolver() = default;
  // 设置相机参数
  void setCameraParams(const cv::Mat &camera_matrix,
                       const cv::Mat &dist_coeffs);
  // 设置物体点
  void setObjectPoints(const std::vector<cv::Point3f> &object_points);
  // 解算位姿
  bool solve(const std::vector<cv::Point2f> &image_points);
  // 映射回图像
  std::vector<cv::Point2f> project(const cv::Mat& rvec, const cv::Mat& tvec);
  // 获取旋转向量
  cv::Mat getRvec() const;
  // 获取平移向量
  cv::Mat getTvec() const;
};